/*
 * ----------------------------------------------------------------------------
 * "THE BEER-WARE LICENSE" (Revision 42):
 * <info@gerhard-bertelsmann.de> wrote this file. As long as you retain this notice you
 * can do whatever you want with this stuff. If we meet some day, and you think
 * this stuff is worth it, you can buy me a beer in return Gerhard Bertelsmann
 * ----------------------------------------------------------------------------
 */

/* Thanks to Stefan Krauss and the Socketcan team
 */

#include <stdio.h>
#include <stdlib.h>
#include <libgen.h>
#include <unistd.h>
#include <string.h>
#include <signal.h>
#include <errno.h>

#include <sys/types.h>
#include <sys/wait.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <netinet/in.h>

#include <linux/can.h>

#define MAXDG   4096	/* maximum datagram size */
#define MAXUDP  16	/* maximum datagram size */

unsigned char udpframe[MAXDG];


void Signal_Handler(sig) /* signal handler function */
{
	switch(sig) {
		case SIGHUP:
			/* rehash the server */
			break;		
		case SIGTERM:
			/* finalize the server */
			exit(0);
			break;		
	}	
}


void print_usage(char *prg) {
	fprintf(stderr, "\nUsage: %s -l <port> -d <port> -i <can interface>\n", prg);
	fprintf(stderr, "   Version 0.1\n");
	fprintf(stderr, "\n");
	fprintf(stderr, "         -l <port>           listening UDP port for the server - default 15731\n");
	fprintf(stderr, "         -d <port>           destination UDP port for the server - default 15730\n");
	fprintf(stderr, "         -b <broadcast_addr> broadcast address - default 255.255.255.255\n");
	fprintf(stderr, "         -i <can int>        can interface - default can0\n");
	fprintf(stderr, "         -f                  running in foreground\n");
	fprintf(stderr, "\n");
	
}

void send_magic_start_60113_frame (int can_socket, int verbose) {

	struct can_frame frame;
	int nbytes;

	frame.can_id = 0x360301UL;
	/* use EFF */
	frame.can_id &= CAN_EFF_MASK;
	frame.can_id |= CAN_EFF_FLAG;
	frame.can_dlc = 5;
	frame.data[0] = 0;
	frame.data[1] = 0;
	frame.data[2] = 0;
	frame.data[3] = 0;
	frame.data[4] = 0x11;
	if ((nbytes = write(can_socket, &frame, sizeof(frame))) != sizeof(frame)) {
		perror("CAN magic 60113 start write __");
	} else {
	if (verbose)
		printf("CAN magic 60113 start write\n");
	}
}


int main(int argc, char **argv) {
        pid_t pid;
	extern int optind, opterr, optopt;
	int opt;
	struct can_frame frame;

	int sa, sc, sb; /* UDP socket , CAN socket, UDP Broadcast Socket */
	struct sockaddr_in saddr, baddr;
	struct sockaddr_can caddr;
	struct ifreq ifr;
	// socklen_t sin_size = sizeof(clientaddr);
	socklen_t caddrlen = sizeof(caddr);

	fd_set readfds;

	int i, s, nbytes, ret;

	int local_port = 15731;
	int destination_port = 15730;
	// int broadcast_address;
	int verbose = 1;
	int background = 1;
	int canid = 0;
	const int on = 1;
	const char rocrail_server[] = "255.255.255.255";
	strcpy(ifr.ifr_name, "can0");
	
	/* prepare udp destination struct with defaults */
	bzero(&baddr,sizeof(baddr));
	baddr.sin_family = AF_INET;
	baddr.sin_port = htons(destination_port);
	if (inet_pton(AF_INET,rocrail_server,&baddr.sin_addr))
		perror("UDP inet_pton error");

	while ((opt = getopt(argc, argv, "l:d:b:i:fv?")) != -1) {
		switch (opt) {
		case 'l':
			local_port = strtoul(optarg, (char **)NULL, 10);
			break;
		case 'd':
			destination_port = strtoul(optarg, (char **)NULL, 10);
			baddr.sin_port = htons(destination_port);
			break;
		case 'b':
			s = inet_pton(AF_INET,optarg,&baddr.sin_addr);
			if (s <= 0) {
				if (s == 0)  {
                   			fprintf(stderr, "Not in presentation format");
               			} else {
					perror("inet_pton error");
				}
				exit(1);
			}
			break;
		case 'i':
			strcpy(ifr.ifr_name, optarg);
			break;


		case 'v':
			verbose = 1;
			break;
		case 'f':
			background = 0;
			break;

		case '?':
			print_usage(basename(argv[0]));
			exit(0);
			break;

		default:
			fprintf(stderr, "Unknown option %c\n", opt);
			print_usage(basename(argv[0]));
			exit(1);
			break;
		}
	}

	if((sa = socket(PF_INET, SOCK_DGRAM, 0)) < 0) {
		perror("inetsocket srror");
		exit(1);
	}

	saddr.sin_family = AF_INET;
	saddr.sin_addr.s_addr = htonl(INADDR_ANY);
	saddr.sin_port = htons(local_port);

	while(bind(sa,(struct sockaddr*)&saddr, sizeof(saddr)) < 0) {
		printf(".");
		fflush(NULL);
		usleep(100000);
	}

	if ((sc = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
		perror("CAN socket error");
		exit(1);
	}

	caddr.can_family = AF_CAN;
	if (ioctl(sc, SIOCGIFINDEX, &ifr) < 0) {
		perror("SIOCGIFINDEX error");
		exit(1);
	}
	caddr.can_ifindex = ifr.ifr_ifindex;

	if (bind(sc, (struct sockaddr *)&caddr, caddrlen) < 0) {
		perror("CAN bind error");
		exit(1);
	}
	
	/* prepare UDP sending socket */
	if ((sb = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
		perror("Send UDP socket error");
		exit(1);
	}
	if (setsockopt(sb, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)) < 0){
		perror("UDP setsockopt error");
		exit(1);
	}

	/* start Maerklin 60113 box */
	send_magic_start_60113_frame(sc, verbose);

	if (background) {
		/* Fork off the parent process */       
		pid = fork();
		if (pid < 0) {
			exit(EXIT_FAILURE);
		}
		/* If we got a good PID, then we can exit the parent process */
		
		if (pid > 0) {
			printf("Going into background ...\n");
			exit(EXIT_SUCCESS);
		}
	}


	while (1) {

		FD_ZERO(&readfds);
		FD_SET(sc, &readfds);
		FD_SET(sa, &readfds);

		ret = select((sc > sa)?sc+1:sa+1, &readfds, NULL, NULL, NULL);

		/* received a CAN frame */
		if (FD_ISSET(sc, &readfds)) {

			if ((nbytes = read(sc, &frame, sizeof(struct can_frame))) < 0) {
				perror("CAN read error");
			} else if (frame.can_id & CAN_EFF_FLAG) { /* only EFF frames are valid */
				/* prepare UDP frame */
				bzero(udpframe,13);
				frame.can_id &= CAN_EFF_MASK;
				udpframe[0] = (frame.can_id >> 24) & 0x000000FF;
				udpframe[1] = (frame.can_id >> 16) & 0x000000FF;
				udpframe[2] = (frame.can_id >>  8) & 0x000000FF;
				udpframe[3] = frame.can_id & 0x000000FF;
				udpframe[4] = frame.can_dlc;
				memcpy(&udpframe[5],&frame.data,frame.can_dlc);
			
				/* send UDP frame */
				s=sendto(sb, udpframe, 13, 0, (struct sockaddr *)&baddr, sizeof(baddr));
				if (s != 13)
					perror("UDP write error");
	
				if (verbose && !background ) {
					printf("->CAN>UDP CANID 0x%06X R",frame.can_id);
					printf(" [%d]", udpframe[4]);
					for (i=5; i<5+frame.can_dlc;i++) {
						printf(" %02x", udpframe[i]);
					}
					printf("\n");
				}
			}
		}

		// received a UDP packet
		if (FD_ISSET(sa, &readfds)) {
			if (read(sa, udpframe, MAXDG) == 13) {
				/* Maerklin UDP Format: always 13 bytes
				 *   byte 0-3 CAN ID
				 *   byte 4 DLC
				 *   byte 5-12 CAN data
				 */
				memcpy(&canid,&udpframe[0],4);
				/* CAN is stored in network Big Endian format */
	                        frame.can_id = ntohl(canid);
				
				frame.can_id &= CAN_EFF_MASK;
				frame.can_id |= CAN_EFF_FLAG;

	                        frame.can_dlc=udpframe[4];

                        	memcpy(&frame.data,&udpframe[5],8);

				/* send CAN frame */
                        	if ((nbytes = write(sc, &frame, sizeof(frame))) != sizeof(frame))
					perror("CAN write error");
	

				if (verbose && !background ) {
					printf("<-UDP>CAN CANID 0x%06X  ",frame.can_id&CAN_EFF_MASK);
					printf(" [%d]", udpframe[4]);
					for (i=5; i<5+frame.can_dlc;i++) {
						printf(" %02x", udpframe[i]);
					}
					printf("\n");
			
				}
			} 
		}
	}
	close(sc);
	close(sa);
	close(sb);
	return 0;
}
